// -*- coding: utf-8 -*-
#include "chassis.h"
#include <locale>
#include <codecvt>

ros::Subscriber state_sub;
ros::Subscriber pose_sub;
ros::Subscriber vel_sub;
ros::Publisher local_pose_pub;
ros::Publisher local_vel_pub;
ros::Publisher state_pub;
ros::Publisher set_raw_pub;
ros::Publisher state_message_pub;
ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;

int land_distance = -1;
bool cur_state;
bool hold_reset_flag = false;
bool rotation_reset_flag = false;
bool velocity_reset_flag = false;
mavros_msgs::State current_state;
geometry_msgs::PoseStamped current_pose;
geometry_msgs::TwistStamped current_vel;

bool state_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
    cur_state = !cur_state;
    ROS_INFO("Publish command [%s]", cur_state == true ? "Yes" : "No");
    res.success = true;
    res.message = "Change drone command state!";
    return true;
}

void reset_hold_state()
{
    hold_reset_flag = true;
}

void reset_rotation_state()
{
    rotation_reset_flag = true;
}

void state_cb(const mavros_msgs::State::ConstPtr &msg)
{
    current_state = *msg;
}

void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    current_pose = *msg;
}

void vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg)
{
    current_vel = *msg;
}

double move_map::func(double error, double A, double B)
{
    double result;
    if (error >= B + A)
    {
        result = 0;
    }
    else if (error < B + A && error > B)
    {
        result = (A + B - error) / A;
    }
    else
    {
        result = 1;
    }
    return result;
}

int move_map::fly_to_target(geometry_msgs::PoseStamped target_pose, ros::Publisher &local_pose_pub, ros::Rate &rate, double speed)
{
    int judge = 0;
    static geometry_msgs::PoseStamped direct_target;
    static bool first_call = true;

    if (first_call)
    {
        direct_target = current_pose;
        first_call = false;
    }

    double ex = target_pose.pose.position.x - current_pose.pose.position.x;
    double ey = target_pose.pose.position.y - current_pose.pose.position.y;
    double ez = target_pose.pose.position.z - current_pose.pose.position.z;

    double error_distance = sqrt(ex * ex + ey * ey + ez * ez);

    double base_move_factor = 0.03 * speed;

    double distance_factor;
    const double breakpoint = 0.7;
    const double max_distance = 3.0;

    // 限制最大距离为3.0m
    double limited_distance = std::min(error_distance, max_distance);

    if (limited_distance <= breakpoint)
    {
        // 0.7m以内：使用线性函数，距离越近速度越慢（精确控制）
        // 在breakpoint处 distance_factor = 1.0，在0距离处 distance_factor = 2.0
        distance_factor = 2.0 - (limited_distance / breakpoint);
        distance_factor = std::min(2.0, std::max(1.0, distance_factor));
    }
    else
    {
        // 0.7m到3.0m：使用对数函数，保证平滑过渡且远距离有合理速度
        // 使用改进的对数函数，确保在breakpoint处连续
        double normalized_distance = (limited_distance - breakpoint) / (max_distance - breakpoint);
        
        // 对数函数设计：在breakpoint处值为1.0，在max_distance处约为0.4
        // 使用 ln(1 + 2*x) 的形式，提供更平滑的衰减
        double log_factor = std::log(1.0 + 2.0 * normalized_distance);
        distance_factor = 1.0 - 0.6 * (log_factor / std::log(3.0)); // log(3) ≈ 1.099
        
        distance_factor = std::min(1.0, std::max(0.4, distance_factor));
    }

    double move_factor_x = base_move_factor * distance_factor;
    double move_factor_y = base_move_factor * distance_factor;
    double move_factor_z = base_move_factor * distance_factor;

    move_factor_x = std::max(0.005, move_factor_x);
    move_factor_y = std::max(0.005, move_factor_y);
    move_factor_z = std::max(0.005, move_factor_z);

    direct_target.header.stamp = ros::Time::now();
    direct_target.header.frame_id = "map";

    direct_target.pose.position.x += move_factor_x * (target_pose.pose.position.x - direct_target.pose.position.x);
    direct_target.pose.position.y += move_factor_y * (target_pose.pose.position.y - direct_target.pose.position.y);
    direct_target.pose.position.z += move_factor_z * (target_pose.pose.position.z - direct_target.pose.position.z);

    if ((target_pose.pose.position.x - current_pose.pose.position.x) *
            (target_pose.pose.position.x - direct_target.pose.position.x) <=
        0)
    {
        direct_target.pose.position.x = target_pose.pose.position.x;
    }
    if ((target_pose.pose.position.y - current_pose.pose.position.y) *
            (target_pose.pose.position.y - direct_target.pose.position.y) <=
        0)
    {
        direct_target.pose.position.y = target_pose.pose.position.y;
    }
    if ((target_pose.pose.position.z - current_pose.pose.position.z) *
            (target_pose.pose.position.z - direct_target.pose.position.z) <=
        0)
    {
        direct_target.pose.position.z = target_pose.pose.position.z;
    }

    if (target_pose.pose.orientation.w == 0.0 &&
        target_pose.pose.orientation.x == 0.0 &&
        target_pose.pose.orientation.y == 0.0 &&
        target_pose.pose.orientation.z == 0.0)
    {
        direct_target.pose.orientation = current_pose.pose.orientation;
    }
    else
    {
        direct_target.pose.orientation = target_pose.pose.orientation;
    }

    local_pose_pub.publish(direct_target);

    static ros::Time last_debug_time;
    static bool debug_time_initialized = false;
    if (!debug_time_initialized)
    {
        last_debug_time = ros::Time::now();
        debug_time_initialized = true;
    }

    if ((ros::Time::now() - last_debug_time).toSec() >= 2.0)
    {
        ROS_INFO("Final Target: [%.3f, %.3f, %.3f], Direct Target: [%.3f, %.3f, %.3f], Current: [%.3f, %.3f, %.3f]",
                 target_pose.pose.position.x, target_pose.pose.position.y, target_pose.pose.position.z,
                 direct_target.pose.position.x, direct_target.pose.position.y, direct_target.pose.position.z,
                 current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
        ROS_INFO("Error Distance: %.3f m, Distance Factor: %.3f (%.1fm breakpoint), Move Factors: x=%.3f, y=%.3f, z=%.3f",
                 error_distance, distance_factor, breakpoint, move_factor_x, move_factor_y, move_factor_z);
        last_debug_time = ros::Time::now();
    }

    static ros::Time target_reached_start_time;
    static bool target_reached_timing = false;
    static bool target_reached_time_initialized = false;

    if (fabs(ex) < 0.1 && fabs(ey) < 0.1 && fabs(ez) < 0.1)
    {
        if (!target_reached_timing)
        {
            target_reached_start_time = ros::Time::now();
            target_reached_timing = true;
            target_reached_time_initialized = true;
            ROS_INFO_THROTTLE(1.0,  "Entered target zone, starting 2-second confirmation timer...");
        }
        else
        {
            double time_in_target = (ros::Time::now() - target_reached_start_time).toSec();
            if (time_in_target >= 1)
            {
                judge = 1;
                first_call = true;
                target_reached_timing = false;
                debug_time_initialized = false;
                ROS_INFO("Target reached! Confirmed for %.2f seconds. Resetting for next waypoint.", time_in_target);
            }
            else
            {
                ROS_INFO_THROTTLE(0.24,  "In target zone for %.2f/1.0 seconds...", time_in_target);
            }
        }
    }
    else
    {
        if (target_reached_timing)
        {
            target_reached_timing = false;
            ROS_INFO("Left target zone, resetting timer. Distance: %.3f m", error_distance);
        }
    }

    return judge;
}

void move_map::pose_velocity_ctrl(double vx, double vy, double vz, double yaw_rate, ros::Rate &rate)
{
    vx *= 18;
    vy *= 18;
    vz *= 18;
    
    double vel_limit_x = 25;
    double vel_limit_y = 25;
    double vel_limit_z = 25;
    double yaw_limit = 0.5;
    
    ros::Time current_time = ros::Time::now();
    double dt = rate.expectedCycleTime().toSec();
    
    vx = std::max(-vel_limit_x, std::min(vx, vel_limit_x));
    vy = std::max(-vel_limit_y, std::min(vy, vel_limit_y));
    vz = std::max(-vel_limit_z, std::min(vz, vel_limit_z));
    yaw_rate = std::max(-yaw_limit, std::min(yaw_rate, yaw_limit));
    
    geometry_msgs::PoseStamped command_pose;
    command_pose.header.stamp = current_time;
    command_pose.header.frame_id = "map";
    
    command_pose.pose.position.x = current_pose.pose.position.x + vx * dt;
    command_pose.pose.position.y = current_pose.pose.position.y + vy * dt;
    command_pose.pose.position.z = current_pose.pose.position.z + vz * dt;
    
    double current_yaw = tf::getYaw(current_pose.pose.orientation);
    double new_yaw = current_yaw + yaw_rate * dt;
    
    tf::Quaternion q_tf = tf::createQuaternionFromYaw(new_yaw);
    geometry_msgs::Quaternion new_orientation;
    tf::quaternionTFToMsg(q_tf, new_orientation);
    command_pose.pose.orientation = new_orientation;
    
    local_pose_pub.publish(command_pose);
    
    ROS_INFO_THROTTLE(1.0, "Velocity ctrl: vx=%.2f vy=%.2f vz=%.2f | target=[%.2f,%.2f,%.2f] | actual=[%.2f,%.2f,%.2f]",
                     vx, vy, vz, 
                     command_pose.pose.position.x, command_pose.pose.position.y, command_pose.pose.position.z,
                     current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
}

void move_map::reset_velocity_control()
{
    velocity_reset_flag = true;
    ROS_INFO("Velocity control reset flag set");
}

void move_map::hold(ros::Publisher &local_pose_pub, ros::Rate &rate)
{
    static geometry_msgs::PoseStamped fixed_hold_pose;
    static bool hold_initialized = false;

    if (hold_reset_flag)
    {
        hold_initialized = false;
        hold_reset_flag = false;
    }

    if (!hold_initialized)
    {
        fixed_hold_pose = current_pose;
        fixed_hold_pose.header.stamp = ros::Time::now();
        fixed_hold_pose.header.frame_id = "map";
        hold_initialized = true;
        ROS_INFO("Hold position locked at [%.3f, %.3f, %.3f]",
                 fixed_hold_pose.pose.position.x,
                 fixed_hold_pose.pose.position.y,
                 fixed_hold_pose.pose.position.z);
    }

    // 使用 fly_to_target 实现平滑悬停
    fly_to_target(fixed_hold_pose, local_pose_pub, rate, 1.0);
}

void move_map::reset_hold()
{
    static bool *hold_initialized_ptr = nullptr;
    if (hold_initialized_ptr == nullptr)
    {
        static bool reset_flag = true;
        if (reset_flag)
        {
            reset_flag = false;
        }
    }
}

int move_map::performPoleSurrounding(float pole_x, float pole_y, float radius, float height, bool clockwise)
{
    float start_angle = 0.0;
    float end_angle = 2 * M_PI;
    float angular_speed = 0.5;
    float pose_limit = 0.1;

    static float current_angle;
    static bool initialized = false;
    static ros::Time last_time;
    static float angle_traveled = 0;
    static float total_angle_to_travel;
    static float prev_raw_angle;
    static float raw_angle;
    static float current_yaw;
    static bool yaw_initialized = false;

    if (!initialized)
    {
        float current_x = current_pose.pose.position.x;
        float current_y = current_pose.pose.position.y;

        start_angle = atan2(current_y - pole_y, current_x - pole_x);

        current_yaw = tf::getYaw(current_pose.pose.orientation);
        yaw_initialized = true;

        float current_distance = sqrt(pow(current_x - pole_x, 2) + pow(current_y - pole_y, 2));

        if (fabs(current_distance - radius) > 0.3)
        {
            ROS_WARN("无人机距离杆子 %.2f 米，目标半径 %.2f 米，正在调整到合适位置",
                     current_distance, radius);

            float target_x = pole_x + radius * cos(start_angle);
            float target_y = pole_y + radius * sin(start_angle);

            geometry_msgs::PoseStamped adjust_target;
            adjust_target.header.frame_id = "map";
            adjust_target.header.stamp = ros::Time::now();
            adjust_target.pose.position.x = target_x;
            adjust_target.pose.position.y = target_y;
            adjust_target.pose.position.z = height;

            float yaw = atan2(pole_y - target_y, pole_x - target_x);
            tf::Quaternion q_tf = tf::createQuaternionFromYaw(yaw);
            geometry_msgs::Quaternion target_q;
            tf::quaternionTFToMsg(q_tf, target_q);
            adjust_target.pose.orientation = target_q;

            local_pose_pub.publish(adjust_target);

            float adjust_dist = sqrt(pow(target_x - current_x, 2) + pow(target_y - current_y, 2));
            if (adjust_dist > 0.2)
            {
                return 0;
            }
        }

        // 根据方向设置结束角度
        end_angle = start_angle + (clockwise ? -2 * M_PI : 2 * M_PI);

        total_angle_to_travel = 2 * M_PI;

        angle_traveled = 0;
        current_angle = start_angle;
        raw_angle = start_angle;
        prev_raw_angle = raw_angle;
        last_time = ros::Time::now();

        ROS_INFO_THROTTLE(1.0, "绕杆飞行初始化: 从无人机当前位置开始，起始角度 %.2f 度，方向: %s，总计角度: %.2f",
                 start_angle * 180.0 / M_PI, clockwise ? "顺时针" : "逆时针", total_angle_to_travel);

        initialized = true;
    }

    ros::Time current_time = ros::Time::now();
    double dt = (current_time - last_time).toSec();
    last_time = current_time;

    if (dt > 0.1)
        dt = 0.1;

    float step_angle = dt * angular_speed;

    prev_raw_angle = raw_angle;

    if (clockwise)
    {
        raw_angle -= step_angle;
    }
    else
    {
        raw_angle += step_angle;
    }

    current_angle = atan2(sin(raw_angle), cos(raw_angle));

    angle_traveled += step_angle;

    if (angle_traveled >= total_angle_to_travel)
    {
        ROS_INFO_THROTTLE(0.50,  "绕杆飞行完成: 已转过 %.2f，目标 %.2f", angle_traveled, total_angle_to_travel);
        initialized = false;
        angle_traveled = 0;
        return 1;
    }

    float px = radius * cos(current_angle) + pole_x;
    float py = radius * sin(current_angle) + pole_y;

    float dist = sqrt(pow(px - current_pose.pose.position.x, 2) +
                      pow(py - current_pose.pose.position.y, 2));

    if (dist > pose_limit)
    {
        float adjustment = step_angle * 0.7;

        if (clockwise)
        {
            raw_angle += adjustment;
        }
        else
        {
            raw_angle -= adjustment;
        }

        current_angle = atan2(sin(raw_angle), cos(raw_angle));

        angle_traveled -= adjustment;

        px = radius * cos(current_angle) + pole_x;
        py = radius * sin(current_angle) + pole_y;

        ROS_INFO_THROTTLE(0.5,  "距离过远 (%.2f m)，减小角度步长", dist);
    }

    geometry_msgs::PoseStamped target_pose;
    target_pose.header.frame_id = "map";
    target_pose.header.stamp = current_time;
    target_pose.pose.position.x = px;
    target_pose.pose.position.y = py;
    target_pose.pose.position.z = height;

    float target_yaw = atan2(pole_y - py, pole_x - px);

    float yaw_error = target_yaw - current_yaw;
    while (yaw_error > M_PI)
        yaw_error -= 2 * M_PI;
    while (yaw_error < -M_PI)
        yaw_error += 2 * M_PI;

    float max_yaw_rate = 0.523;
    float max_yaw_step = max_yaw_rate * dt;

    if (fabs(yaw_error) > max_yaw_step)
    {
        current_yaw += (yaw_error > 0) ? max_yaw_step : -max_yaw_step;
    }
    else
    {
        current_yaw = target_yaw;
    }

    current_yaw = atan2(sin(current_yaw), cos(current_yaw));

    tf::Quaternion q_tf = tf::createQuaternionFromYaw(current_yaw);
    geometry_msgs::Quaternion target_q;
    tf::quaternionTFToMsg(q_tf, target_q);
    target_pose.pose.orientation = target_q;

    local_pose_pub.publish(target_pose);

    float completion_percent = 100.0 * angle_traveled / total_angle_to_travel;

    ROS_INFO_THROTTLE(1.0, "绕杆进行中: 当前角度 %.2f, 原始角度 %.2f, 目标角度 %.2f, 已转过 %.2f/%.2f (%.1f%%), 偏航角 %.1f°→%.1f°",
                      current_angle, raw_angle, end_angle, angle_traveled, total_angle_to_travel,
                      completion_percent, current_yaw * 180.0 / M_PI, target_yaw * 180.0 / M_PI);

    return 0;
}

bool move_map::performRotation(double targetYaw, ros::Rate &rate)
{
    static double lastTargetYaw = -999.0;
    static double currentTargetYaw = 0.0;
    static geometry_msgs::PoseStamped fixedPosition;
    static bool rotationInitialized = false;

    double currentYaw = tf::getYaw(current_pose.pose.orientation);
    double yawError = targetYaw - currentYaw;

    while (yawError > M_PI)
        yawError -= 2 * M_PI;
    while (yawError < -M_PI)
        yawError += 2 * M_PI;

    if (rotation_reset_flag)
    {
        rotationInitialized = false;
        rotation_reset_flag = false;
    }

    if (!rotationInitialized || fabs(targetYaw - lastTargetYaw) > 0.01)
    {
        fixedPosition = current_pose;
        fixedPosition.header.stamp = ros::Time::now();
        fixedPosition.header.frame_id = "map";
        currentTargetYaw = currentYaw;
        rotationInitialized = true;
        lastTargetYaw = targetYaw;

        ROS_INFO("Rotation position locked at [%.3f, %.3f, %.3f]",
                 fixedPosition.pose.position.x, fixedPosition.pose.position.y, fixedPosition.pose.position.z);
        ROS_INFO("Starting rotation from %.2f to %.2f degrees",
                 currentYaw * 180.0 / M_PI, targetYaw * 180.0 / M_PI);
    }

    double dt = rate.expectedCycleTime().toSec();
    double maxRotationStep = 0.523 * dt;

    double remainingError = targetYaw - currentTargetYaw;
    while (remainingError > M_PI)
        remainingError -= 2 * M_PI;
    while (remainingError < -M_PI)
        remainingError += 2 * M_PI;

    if (fabs(remainingError) > maxRotationStep)
    {
        currentTargetYaw += (remainingError > 0) ? maxRotationStep : -maxRotationStep;
    }
    else
    {
        currentTargetYaw = targetYaw;
    }

    geometry_msgs::PoseStamped rotation_pose;
    rotation_pose.header.stamp = ros::Time::now();
    rotation_pose.header.frame_id = "map";

    rotation_pose.pose.position.x = fixedPosition.pose.position.x;
    rotation_pose.pose.position.y = fixedPosition.pose.position.y;
    rotation_pose.pose.position.z = fixedPosition.pose.position.z;

    tf::Quaternion q_tf = tf::createQuaternionFromYaw(currentTargetYaw);
    geometry_msgs::Quaternion target_q;
    tf::quaternionTFToMsg(q_tf, target_q);
    rotation_pose.pose.orientation = target_q;

    local_pose_pub.publish(rotation_pose);

    if (fabs(yawError) < 0.047)
    {
        rotationInitialized = false;
        lastTargetYaw = -999.0;
        ROS_INFO("Rotation completed! Final yaw: %.2f degrees at fixed position [%.3f, %.3f, %.3f]",
                 currentYaw * 180.0 / M_PI,
                 fixedPosition.pose.position.x, fixedPosition.pose.position.y, fixedPosition.pose.position.z);
        return true;
    }

    static ros::Time last_debug_time;
    static bool debug_initialized = false;
    if (!debug_initialized)
    {
        last_debug_time = ros::Time::now();
        debug_initialized = true;
    }

    if ((ros::Time::now() - last_debug_time).toSec() >= 2.0)
    {
        ROS_INFO("Rotating: current=%.1f°, target=%.1f°, error=%.1f°",
                 currentYaw * 180.0 / M_PI, targetYaw * 180.0 / M_PI, yawError * 180.0 / M_PI);
        ROS_INFO("Fixed position: [%.3f, %.3f, %.3f], Current position: [%.3f, %.3f, %.3f]",
                 fixedPosition.pose.position.x, fixedPosition.pose.position.y, fixedPosition.pose.position.z,
                 current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
        last_debug_time = ros::Time::now();
    }

    return false;
}

int move_map::performCircleFlight(double centerX, double centerY, double centerZ, double radius, double elapsedTime, ros::Rate &rate)
{
    double angularSpeed = 0.5;
    double angle = angularSpeed * elapsedTime;

    double targetX = centerX + radius * cos(angle);
    double targetY = centerY + radius * sin(angle);

    geometry_msgs::PoseStamped target;
    target.pose.position.x = targetX;
    target.pose.position.y = targetY;
    target.pose.position.z = centerZ;

    return fly_to_target(target, local_pose_pub, rate, 1.0);
}

bool move_map::performSafeLanding(double descent_rate, ros::Rate &rate)
{
    static double landing_target_height = 0.0;
    static bool landing_initialized = false;
    static bool auto_land_triggered = false;
    static geometry_msgs::PoseStamped fixed_xy_position;

    if (!landing_initialized)
    {
        landing_target_height = current_pose.pose.position.z;
        landing_initialized = true;
        auto_land_triggered = false;
        
        // 锁定初始 xy 位置，只在 z 方向下降
        fixed_xy_position = current_pose;
        fixed_xy_position.header.frame_id = "map";
        
        ROS_WARN("Starting safe landing from %.2f meters at fixed position [%.3f, %.3f]", 
                 landing_target_height, fixed_xy_position.pose.position.x, fixed_xy_position.pose.position.y);
    }

    if (!auto_land_triggered)
    {
        double dt = rate.expectedCycleTime().toSec();
        landing_target_height -= descent_rate * dt;

        if (landing_target_height < 0.13)
        {
            landing_target_height = 0.10;
        }

        geometry_msgs::PoseStamped landing_target;
        landing_target.header.stamp = ros::Time::now();
        landing_target.header.frame_id = "map";
        // 使用固定的 xy 位置，只改变 z 高度
        landing_target.pose.position.x = fixed_xy_position.pose.position.x;
        landing_target.pose.position.y = fixed_xy_position.pose.position.y;
        landing_target.pose.position.z = landing_target_height;
        landing_target.pose.orientation = fixed_xy_position.pose.orientation;

        int landing_result = fly_to_target(landing_target, local_pose_pub, rate, 0.9);

        static ros::Time last_landing_debug_time;
        static bool landing_debug_initialized = false;
        if (!landing_debug_initialized)
        {
            last_landing_debug_time = ros::Time::now();
            landing_debug_initialized = true;
        }

        if ((ros::Time::now() - last_landing_debug_time).toSec() >= 2.0)
        {
            ROS_INFO("Landing: Current height: %.2fm, Target height: %.2fm, Speed: %.2fm/s",
                     current_pose.pose.position.z, landing_target_height, descent_rate);
            ROS_INFO("Landing position locked at: [%.3f, %.3f], Current xy: [%.3f, %.3f]",
                     fixed_xy_position.pose.position.x, fixed_xy_position.pose.position.y,
                     current_pose.pose.position.x, current_pose.pose.position.y);
            last_landing_debug_time = ros::Time::now();
        }

        if (current_pose.pose.position.z <= 0.15)
        {
            auto_land_triggered = true;
            landing_debug_initialized = false;
            ROS_WARN("Switching to AUTO.LAND at %.2f meters", current_pose.pose.position.z);
        }

        return false;
    }

    static ros::Time land_request_time = ros::Time::now();
    mavros_msgs::SetMode land_mode;
    land_mode.request.custom_mode = "AUTO.LAND";

    if (current_state.mode != "AUTO.LAND" &&
        (ros::Time::now() - land_request_time > ros::Duration(2.0)))
    {
        if (set_mode_client.call(land_mode) && land_mode.response.mode_sent)
        {
            ROS_INFO("AUTO.LAND enabled for final touchdown");
            landing_initialized = false;
            auto_land_triggered = false;
            return true;
        }
        land_request_time = ros::Time::now();
    }

    return false;
}

bool move_map::isReachedTarget(double target_x, double target_y, double target_z, double tolerance)
{
    double ex = target_x - current_pose.pose.position.x;
    double ey = target_y - current_pose.pose.position.y;
    double ez = target_z - current_pose.pose.position.z;
    ROS_INFO_THROTTLE(1.0, "[isReachedTarget]Checking target reach: ex=%.2f, ey=%.2f, ez=%.2f", ex, ey, ez);
    return (fabs(ex) < tolerance && fabs(ey) < tolerance && fabs(ez) < tolerance);
}

// 新增：Offboard模式启用和解锁功能
bool move_map::performOffboardEnable(ros::Rate &rate)
{
    static bool offboard_initialized = false;
    static ros::Time offboard_start_time;
    static bool position_stream_started = false;
    
    // 1. 首先发送位置目标流以满足offboard模式要求
    if (!position_stream_started) {
        // 发送当前位置作为目标位置
        geometry_msgs::PoseStamped pose;
        pose.pose.position.x = current_pose.pose.position.x;
        pose.pose.position.y = current_pose.pose.position.y;
        pose.pose.position.z = current_pose.pose.position.z;
        pose.pose.orientation = current_pose.pose.orientation;
        
        local_pose_pub.publish(pose);
        
        ROS_INFO_THROTTLE(0.5, "[OFFBOARD_ENABLE] 发送位置流准备offboard模式...");
        
        // 发送位置流2秒后再尝试切换模式
        if (!offboard_initialized) {
            offboard_start_time = ros::Time::now();
            offboard_initialized = true;
        }
        
        if (ros::Time::now() - offboard_start_time > ros::Duration(2.0)) {
            position_stream_started = true;
        }
        
        rate.sleep();
        return false;
    }
    
    // 2. 检查是否已经在offboard模式且已解锁
    if (isOffboardEnabled() && isVehicleArmed()) {
        ROS_INFO("[OFFBOARD_ENABLE] Offboard模式已启用且飞机已解锁");
        // 重置静态变量为下次使用
        offboard_initialized = false;
        position_stream_started = false;
        return true;
    }
    
    // 3. 尝试启用offboard模式
    if (!isOffboardEnabled()) {
        mavros_msgs::SetMode offb_set_mode;
        offb_set_mode.request.custom_mode = "OFFBOARD";
        
        if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
            ROS_INFO("[OFFBOARD_ENABLE] Offboard模式请求已发送");
        } else {
            ROS_WARN("[OFFBOARD_ENABLE] 无法设置Offboard模式");
        }
    }
    
    // 4. 尝试解锁飞机
    if (isOffboardEnabled() && !isVehicleArmed()) {
        mavros_msgs::CommandBool arm_cmd;
        arm_cmd.request.value = true;
        
        if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
            ROS_INFO("[OFFBOARD_ENABLE] 飞机解锁请求已发送");
        } else {
            ROS_WARN("[OFFBOARD_ENABLE] 无法解锁飞机");
        }
    }
    
    // 继续发送位置流保持offboard模式
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = current_pose.pose.position.x;
    pose.pose.position.y = current_pose.pose.position.y;
    pose.pose.position.z = current_pose.pose.position.z;
    pose.pose.orientation = current_pose.pose.orientation;
    local_pose_pub.publish(pose);
    
    rate.sleep();
    return false;
}

bool move_map::isOffboardEnabled()
{
    return (current_state.mode == "OFFBOARD");
}

bool move_map::isVehicleArmed()
{
    return current_state.armed;
}
